#include "main.h"
#include "Sensor.h" 
#include "bsp_as5600.h"
/******************************************************************************/
MagnetSensor_Para  AS5047P_para,TLE5012B_para,MA730_para;
/******************************************************************************/
long  cpr;
float full_rotation_offset;
float angle_prev;
unsigned long velocity_calc_timestamp;
/******************************************************************************/

/******************************************************************************/
  //初始化I2C接口和初始化原始角度
void MagneticSensor_Init(void)
{ 
	HAL_Delay(10);
	velocity_calc_timestamp=0;//SysTick->VAL;
	HAL_Delay(5);
	angle_prev = getAngle();
}

// Shaft velocity calculation
float  getVelocity(void)
{
	unsigned long now_us;
	float  Ts, angle_now, vel;

	// calculate sample time
	now_us = SysTick->VAL; //_micros();
	if(now_us<velocity_calc_timestamp)Ts = (float)(velocity_calc_timestamp - now_us)/9*1e-6;
	else
		Ts = (float)(0xFFFFFF - now_us + velocity_calc_timestamp)/9*1e-6;
	// quick fix for strange cases (micros overflow)
	if(Ts == 0 || Ts > 0.5) Ts = 1e-3;

	// current angle
	angle_now = getAngle();
	// velocity calculation
	vel = (angle_now - angle_prev)/Ts;
	
	// save variables for future pass
	angle_prev = angle_now;
	velocity_calc_timestamp = now_us;
	return vel;
}
/******************************************************************************/


